#include <stdio.h>
#include "os/queue.h"
#include "os/os.h"
#include "bsp/cpu_driver.h"
#include "utils/utils.h"
#include "os/queue.h"
#include "communication/message.h"

#define CAN_RX_MESSAGE_RX_ID 1
#define CAN_SEND_QUEUE_SIZE 32
#define RX_ID_OFFSET 16
#define CAN_SEND_OK 0
#define CAN_SEND_ERROR -1
#define CAN_SEND_WAIT_TIMEOUT -2


#define TX_NUM 64
#define RX_NUM 64
static co_queue_t g_tx_queue;
static co_queue_t g_rx_queue;
static uint8_t _g_tx_buffer[sizeof(can_tx_message_type) * TX_NUM + 1];
static uint8_t _g_rx_buffer[sizeof(can_rx_message_type) * RX_NUM + 1];
static can_frame_handler _handler = NULL;

static uint8_t can_get_mailbox(uint32_t can_periph);

void can_rx_poll(void){
	can_rx_message_type message;
	if (queue_get(&g_rx_queue, &message) && (_handler != NULL)) {
		_handler(message.extended_id, message.data, message.dlc);
	}
}

void can_tx_poll(void){
	can_tx_message_type can_tr_m;
	while (can_get_mailbox((u32)CAN1) != CAN_TX_STATUS_NO_EMPTY) {
		if (!queue_get(&g_tx_queue, &can_tr_m)) {
			break;
		}
		can_message_transmit(CAN1,&can_tr_m);
	}
}

bool can_dev_get_error(void)
{
	__IO uint32_t err_index = 0;

	if(can_flag_get(CAN1, CAN_ETR_FLAG) == RESET) {
		return false;
	}

	err_index = CAN1->ests & 0x70;
	can_flag_clear(CAN1, CAN_ETR_FLAG);

	/* error type is stuff error */
	return (err_index == 0x00000010);
}


static u32 _can_poll_task(void *args) {
	if (can_dev_get_error()) {
		can_drv_reset();
	}
	can_rx_poll();
	can_tx_poll();
	return 0;
}

static __inline__ void can_fifo_recv(can_rx_fifo_num_type fifo){
	can_rx_message_type Rxmessage;

	can_message_receive(CAN1, fifo, &Rxmessage);

	queue_put(&g_rx_queue, &Rxmessage);

}

void USBFS_L_CAN1_RX0_IRQHandler(void)
{
	can_fifo_recv(CAN_RX_FIFO0);
}

void CAN1_RX1_IRQHandler(void)
{
	can_fifo_recv(CAN_RX_FIFO1);
}

static void can_io_init(void){

#ifdef CAN_REMAP
	gpio_pin_remap_config(CAN_REMAP,TRUE);
#endif	
	gpio2_init_can(CAN_RX_PIO, CAN_TX_PIO);
}


static void can_dev_filter_init(u8 index, can_filter_fifo_type fifo, u8 dest) // must use can0
{
	can_filter_init_type can_filter_init_struct;

	can_filter_init_struct.filter_activate_enable = TRUE;
	can_filter_init_struct.filter_mode = CAN_FILTER_MODE_ID_MASK;
	can_filter_init_struct.filter_fifo = fifo;
	can_filter_init_struct.filter_number = index;
	can_filter_init_struct.filter_bit = CAN_FILTER_32BIT;
	can_filter_init_struct.filter_id_high = 0x0000;	/* extended identifier is 29 bit */
	can_filter_init_struct.filter_id_low = dest << 3 | 1 << 2;
	can_filter_init_struct.filter_mask_high = 0x0000; /* extended identifier is 29 bit */
	can_filter_init_struct.filter_mask_low = CAN_FILTER_DEST_MASK << 3 | 1 << 2;
	can_filter_init(CAN1, &can_filter_init_struct);
}


static void can_config(void)
{
	can_base_type can_base_struct;
	can_baudrate_type can_baudrate_struct;
	can_filter_init_type can_filter_init_struct;
	
	crm_periph_clock_enable(CRM_CAN1_PERIPH_CLOCK, TRUE);
	/* can base init */
	can_default_para_init(&can_base_struct);
	can_base_struct.mode_selection = CAN_MODE_COMMUNICATE;
	can_base_struct.ttc_enable = FALSE;
	can_base_struct.aebo_enable = TRUE;
	can_base_struct.aed_enable = TRUE;
	can_base_struct.prsf_enable = FALSE;
	can_base_struct.mdrsel_selection = CAN_DISCARDING_FIRST_RECEIVED;
	can_base_struct.mmssr_selection = CAN_SENDING_BY_REQUEST;
	can_base_init(CAN1, &can_base_struct);

	//CAN0_SPEED==250 //250k bps
	/* can baudrate, set baudrate = pclk/(baudrate_div *(1 + bts1_size + bts2_size)) */
	can_baudrate_struct.baudrate_div = 50;
	can_baudrate_struct.rsaw_size = CAN_RSAW_1TQ;
	can_baudrate_struct.bts1_size = CAN_BTS1_6TQ;
	can_baudrate_struct.bts2_size = CAN_BTS2_1TQ;
	can_baudrate_set(CAN1, &can_baudrate_struct);

	/* can filter 0 config */
	can_dev_filter_init(0, CAN_FILTER_FIFO0, MY_MESSAGE_ADDR);
 
  	nvic_irq_enable(USBFS_L_CAN1_RX0_IRQn, CAN_IRQ_PRIORITY, 0);
	nvic_irq_enable(CAN1_RX1_IRQn, CAN_IRQ_PRIORITY, 0);
    can_interrupt_enable(CAN1, CAN_RF0MIEN_INT, TRUE);
	can_interrupt_enable(CAN1, CAN_RF1MIEN_INT, TRUE);
}


static int can_send_data(can_tx_message_type *P_message){
	can_tx_poll();
	if (queue_put(&g_tx_queue, P_message)){
		return CAN_SEND_OK;
	}
	return CAN_SEND_ERROR;
}


static uint8_t can_get_mailbox(uint32_t can_periph)
{
    uint8_t transmit_mailbox = CAN_TX_MAILBOX0;
    can_type* can_x = CAN1;
	
	/* select one empty transmit mailbox */
	if(can_x->tsts_bit.tm0ef)
	{
	  transmit_mailbox = CAN_TX_MAILBOX0;
	}
	else if(can_x->tsts_bit.tm1ef)
	{
	  transmit_mailbox = CAN_TX_MAILBOX1;
	}
	else if(can_x->tsts_bit.tm2ef)
	{
	  transmit_mailbox = CAN_TX_MAILBOX2;
	}
	else
	{
	  transmit_mailbox = CAN_TX_STATUS_NO_EMPTY;
	}

    /* return no mailbox empty */
    if(CAN_TX_STATUS_NO_EMPTY == transmit_mailbox){
        return CAN_TX_STATUS_NO_EMPTY;
    }
	return transmit_mailbox;
}


int can_send_message(uint32_t can_id, const void*buff, int len){
	can_tx_message_type trasnmit_msg;
	head_t can_frame_id;
	u32 total_frames = ((len + 7) >> 3);
	int send_len = len;
	u32 frame_id = 1;
	can_frame_id.id = can_id;
	can_frame_id.total = total_frames;

	while(send_len > 0){
		can_frame_id.idx = frame_id;

		trasnmit_msg.standard_id	= 0;
		trasnmit_msg.extended_id	= can_frame_id.id;				
		trasnmit_msg.frame_type	= CAN_TFT_DATA;
		trasnmit_msg.id_type	= CAN_ID_EXTENDED;
		trasnmit_msg.dlc = _min(8,send_len);
		memcpy((char *)trasnmit_msg.data, (char *)buff + (len - send_len), trasnmit_msg.dlc);
		send_len -= trasnmit_msg.dlc;
		frame_id ++;
		if (can_send_data(&trasnmit_msg) != CAN_SEND_OK){
			return CAN_SEND_ERROR;
		}
	}

	return CAN_SEND_OK;

}

int can_send_ext_message(uint32_t can_id, const void*buff, int len){
	can_tx_message_type trasnmit_msg;
	
	trasnmit_msg.standard_id	= 0;
	trasnmit_msg.extended_id	= can_id;				
	trasnmit_msg.frame_type	= CAN_TFT_DATA;
	trasnmit_msg.id_type	= CAN_ID_EXTENDED;
	trasnmit_msg.dlc = _min(8,len);
	memcpy((char *)trasnmit_msg.data, (char *)buff, trasnmit_msg.dlc);

	if (can_send_data(&trasnmit_msg) != CAN_SEND_OK){
		return CAN_SEND_ERROR;
	}
	return CAN_SEND_OK;
}


void can_drv_reset(void){
	can_io_init();
	can_config();
}

void can_drv_deinit(void){
	can_reset(CAN1);
	crm_periph_clock_enable(CRM_CAN1_PERIPH_CLOCK, FALSE);
}

void can_drv_init(can_frame_handler handler){
	_handler = handler;
	g_tx_queue = queue_create_with_buffer(_g_tx_buffer, sizeof(_g_tx_buffer), sizeof(can_tx_message_type));
	g_rx_queue = queue_create_with_buffer(_g_rx_buffer, sizeof(_g_rx_buffer), sizeof(can_rx_message_type));

	os_task_create(_can_poll_task, NULL);
	can_io_init();
	can_config();
}

